﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using CommonClasses;
using Descriptions_PinsDescription.Model;

namespace StepperModule
{

    /// <summary>
    /// Модуль для реализации работы портального робота
    /// </summary>
    public class StepperModule : IModule
    {
        #region CONST
        /// <summary>
        /// Название устройства 
        /// </summary>
        private const string usbName = "LEDControl";

        /// <summary>
        /// Дескриптор команды Stepper.Go
        /// </summary>
        private const string GO_DESKR = "Stepper.Go";

        /// <summary>
        /// Дескриптор команды НатыркатьНа
        /// </summary>
        private const string QUERY_DESKR = "Stepper.Query";

        /// <summary>
        /// Дескриптор команды Натыркать влево
        /// </summary>
        private const string STOP_DESKR = "Stepper.Stop";

        /// <summary>
        /// Дескриптор пина обратной связи
        /// </summary>
        private const string BACKPIN_DESKR = "Stepper.InfoPin";

        #region Phisical Params

        /// <summary>
        /// Длина направляющих (максимальная по модулю координата)
        /// </summary>
        const double X_MAX = 90;

        /// <summary>
        /// Диаметр шкива
        /// </summary>
        const double SHKIV_DIAMETER = 4.371;

        /// <summary>
        /// Градусов за один шаг (импульс)
        /// </summary>
        const double DEGREES_PER_STEP = 0.1125;

        /// <summary>
        /// Максимальная скорость контроллера в импульсах
        /// </summary>
        const int IMP_PER_SECOND_MAX = 2475;
        #endregion
        #endregion

        #region Types
        /// <summary>
        /// Тип движения
        /// </summary>
        private enum GoKind
        {
            /// <summary>
            /// Вправо
            /// </summary>
            Right,

            /// <summary>
            /// Влево
            /// </summary>
            Left,

            /// <summary>
            /// В точку
            /// </summary>
            ToPoint
        }

        /// <summary>
        /// Тип режима исполнения
        /// </summary>
        private enum Mode
        {
            /// <summary>
            /// Движемся
            /// </summary>
            Run,
            /// <summary>
            /// Стоим
            /// </summary>
            Stop
        }
        #endregion

        #region Private Fields

        /// <summary>
        /// Устройство
        /// </summary>
        private StepperProvider.StepperProvider stepperProvider = null;

        /// <summary>
        /// Интервал работы таймера
        /// </summary>
        private int tmrInterval;

        /// <summary>
        /// Режим исполнения
        /// </summary>
        private Mode mode = Mode.Stop;

        /// <summary>
        /// Натыркивание
        /// </summary>
        private int query = 0;

        #region Pin Lists
        /// <summary>
        /// Пин, по которому отправляется команда GO
        /// </summary>
        private int goPin;

        /// <summary>
        /// Пин для натыркивания 
        /// </summary>
        private int queryPin;

        /// <summary>
        /// Пин, на который сообщать о текущей координате
        /// </summary>
        private int backPin;

        /// <summary>
        /// Пин для остановки робота
        /// </summary>
        private int stopPin;
        #endregion

        #region Request Params
        /// <summary>
        /// Cмещение, на которое нужно сдвинуться
        /// </summary>
        private double deltaxUnit_req;

        /// <summary>
        /// Cмещение, на которое нужно сдвинуться
        /// </summary>
        private int deltaxImp_req;

        /// <summary>
        /// Ускорение старта
        /// </summary>
        private double aStartUnit_req = 7;

        /// <summary>
        /// Ускорение старта
        /// </summary>
        private int aStartImp_req;

        /// <summary>
        /// Ускорение торможения
        /// </summary>
        private double aFinishUnit_req = 7;

        /// <summary>
        /// Ускорение торможения
        /// </summary>
        private int aFinishImp_req;

        /// <summary>
        /// Маскимальная скорость
        /// </summary>
        private double maxSpeedUnit_req = 10;

        /// <summary>
        /// Маскимальная скорость
        /// </summary>
        private int maxSpeedImp_req;
        #endregion

        #region Debug Params
        /// <summary>
        /// Теоритическое X запросов [ед]
        /// </summary>
        private double teorxUnit;

        /// <summary>
        /// Теоритическое X запросов [имп]
        /// </summary>
        private double teorxImp;

        /// <summary>
        /// Посланное X
        /// </summary>
        private double sentxUnit;

        /// <summary>
        /// Посланное X 
        /// </summary>
        private int sentxImp;


        /// <summary>
        /// Скорость, которая сейчас есть
        /// </summary>
        private int vCur;

        /// <summary>
        /// Предыдущая скорость
        /// </summary>
        private int vLast;

        /// <summary>
        /// Список скоростей на каждый тик таймера
        /// </summary>
        private List<int> speeds = new List<int>();

        /// <summary>
        /// Список шагов на каждый тик таймера
        /// </summary>
        private List<int> stepCounts = new List<int>();

        /// <summary>
        /// Индекс текущей скорости в массивве скоростей движения
        /// </summary>
        private int curSpeedIndex;

        /// <summary>
        /// Количество выпущенных импульсов при заданном запросе
        /// </summary>
        private int stepCountMade;

        /// <summary>
        /// Количество импульсов, которые должен 
        /// </summary>
        private int stepCountMade_new;

        #endregion

        #endregion



        /// <summary>
        /// Перевести число в единицах в импульсы
        /// </summary>
        /// <param name="units">единицы</param>
        /// <returns></returns>
        private double UnitsToImp(double units)
        {
            return (units * 360) / (Math.PI * SHKIV_DIAMETER * DEGREES_PER_STEP);
        }


        /// <summary>
        /// Перевести число в импульсах в единицы
        /// </summary>
        /// <param name="imp">число импульсов</param>
        /// <returns></returns>
        private double ImpToUnits(double imp)
        {
            return (imp * (Math.PI * SHKIV_DIAMETER * DEGREES_PER_STEP)) / 360;
        }



        /// <summary>
        /// Создать модуль для управления портальным роботом
        /// </summary>
        /// <param name="pinTable">таблица описаний всех пинов</param>
        /// <param name="sysTimerInterval">интервал системного таймера
        /// <remarks>для изменения скорости тележки в real-time</remarks>
        /// </param>
        public StepperModule(PinsDescriptionTable pinTable, int sysTimerInterval)
        {
            stepperProvider = new StepperProvider.StepperProvider(usbName);
            tmrInterval = sysTimerInterval;

            FillPinsLists(pinTable);
        }

        /// <summary>
        /// Распознать предназначенные для шаговика пины
        /// </summary>
        /// <param name="pinTable">таблица пинов</param>
        private void FillPinsLists(PinsDescriptionTable pinTable)
        {
            var goPins = from pd in pinTable.PinTable
                         where (pd.Descriptor == GO_DESKR)
                         select pd;

            var backPins = from pd in pinTable.PinTable
                           where (pd.Descriptor == BACKPIN_DESKR)
                           select pd;

            var queryPins = from pd in pinTable.PinTable
                                where (pd.Descriptor == QUERY_DESKR)
                                select pd;
            var stopPins = from pd in pinTable.PinTable
                           where (pd.Descriptor == STOP_DESKR)
                           select pd;

            if (goPins.Count() > 1)
                throw new Exception("There should be one GO_DESKR pin for stepper in pinTable");
            else goPin = goPins.First().VirtualPin;

            if (backPins.Count() > 1)
                throw new Exception("There should be one BACKPIN_COMMAND_DESKR pin for stepper in pinTable");
            else backPin = backPins.First().VirtualPin;

            if (queryPins.Count() > 1)
                throw new Exception("There should be one QUERY_LEFT_DESKR pin for stepper in pinTable");
            else queryPin = queryPins.First().VirtualPin;

            if (stopPins.Count() > 1)
                throw new Exception("There should be one STOP_DESKR pin for stepper in pinTable");
            else stopPin = stopPins.First().VirtualPin;
        }

        /// <summary>
        /// Считать запрашиваемые параметры
        /// </summary>
        /// <param name="deltaX">запрос сдвинуться</param>
        /// <param name="goKind">тип движения</param>
        private void SetRequestParams(int deltaX, GoKind goKind)
        {
            deltaxUnit_req = Math.Abs(deltaX);
            if (goKind == GoKind.Left)
            {
                deltaxUnit_req = Math.Max(-teorxUnit, -deltaxUnit_req);
            }
            else if (goKind == GoKind.Right)
            {
                deltaxUnit_req = Math.Min(X_MAX - teorxUnit, deltaxUnit_req);
            }
            else if (goKind == GoKind.ToPoint)
            {
                deltaxUnit_req = deltaxUnit_req - teorxUnit;
            }
        }

        /// <summary>
        /// Произвести пребразования входных параметров на язык контроллера
        /// </summary>
        /// <param name="tmrInterval">период таймера</param>
        private void TranslateRequestParams(int tmrInterval)
        {
            deltaxImp_req = (int)UnitsToImp(deltaxUnit_req);
            aStartImp_req = (int)((double)(UnitsToImp(aStartUnit_req) * tmrInterval / 1000));
            aFinishImp_req = (int)((double)(UnitsToImp(aFinishUnit_req) * tmrInterval / 1000));
            maxSpeedImp_req = (int)UnitsToImp(maxSpeedUnit_req);
        }

        /// <summary>
        /// Вычислить значения скоростей на каждом этапе движения
        /// </summary>
        /// <param name="stepCount">Количество шагов, на которое расчитывать поездку</param>
        /// <param name="tmrInterval">Интервал работы таймера</param>
        private void EvalSpeedsArray(int stepCount, int tmrInterval)
        {
            speeds.Clear();
            stepCounts.Clear();
            int stepCountMade = 0; vLast = 0;
            do
            {
                vCur = vLast;
                stepCountMade += (int)(vCur * (double)tmrInterval / 1000);

                // Скорость, если захотим ускориться 
                int vAcc = (new int[] { vCur + aStartImp_req, maxSpeedImp_req, IMP_PER_SECOND_MAX }).Min();

                int n = (vCur / aFinishImp_req);

                // Есть три варинанта скорости на след. шаге:
                // 1. Ускориться
                // 2. Не изменить скорость
                // 3. Начать тормозить

                // Не успеем затормозить если ускоримся?
                if (stepCountMade + (vAcc + (n - 1) * vCur - (n * (n - 1) / 2) * aFinishImp_req) * (double)tmrInterval / 1000 > stepCount)
                    // неужели  и сохранить скорость никак?
                    if (stepCountMade + ((n) * vCur - (n * (n - 1) / 2) * aFinishImp_req) * (double)tmrInterval / 1000 > stepCount)
                        vCur = Math.Max(vLast - aFinishImp_req, aFinishImp_req);
                    else vCur = vLast;
                else vCur = vAcc;

                speeds.Add(vCur);
                stepCounts.Add(stepCountMade);
                //usbProvider.SetSpeed(vCur);
                Console.WriteLine("EvalSpeed = {0}", vCur);

                vLast = vCur;

                Console.WriteLine("StepCountMade_eval = {0}", stepCountMade.ToString());
            }
            while (stepCountMade <= stepCount);
        }

        #region IModule Members

        /// <summary>
        /// На Tick'e системного таймера опросить текущее состояние датчиков
        /// <remarks>
        /// При измении какого-либо из них, вызвать событие
        /// </remarks>
        /// </summary>
        /// <param name="sender">object</param>
        /// <param name="e">EventArgs</param>
        public void OnTick(object sender, EventArgs e)
        {
            if (mode == Mode.Stop)
                return;

            /* The old version of evaluating speed beforehand
            //Console.WriteLine("CurTime = " + DateTime.Now.ToString("hh:mm:ss.fff"));
            vCur = speeds[curSpeedIndex];
            stepperProvider.SetSpeed(vCur);
            //log.WriteLine("Speed = " + vCur);
            if (curSpeedIndex < speeds.Count - 1)
                curSpeedIndex++;
             */

            bool params_error = false;

            #region GetParams
            byte[] bytes = stepperProvider.GetParams();
            // затем будем отсюда управлять 
            int n = bytes.GetLength(0);

            // проверка на корректность параметров (не все ли нули)
            int i = 0;
            for (i = 0; i < n; i++)
                if (bytes[i] != 0)
                    break;

            string s = "";
            if ((i < n) && (bytes[i] != 0))
            {
                // 1) state (byte)
                // 2) dir   (byte) 
                // 3) pcur     (int) 
                // 4) stepCountMade (int)
                // 5) impulsesFromLeft (int)

                s += bytes[0].ToString() + ' ';
                s += bytes[1].ToString() + ' ';
                s += ((bytes[2] + ((int)bytes[3] << 8))).ToString() + ' ';
                s += ((bytes[4] + ((int)bytes[5] << 8))).ToString() + ' ';
                s += ((bytes[6] + ((int)bytes[7] << 8))).ToString() + ' ';

                stepCountMade_new = (bytes[4] + ((int)bytes[5] << 8));
            }
            else
            {
                s = "Некорректные параметры";
                params_error = true;
            }

            Console.WriteLine(s);
            #endregion

            if ((stepCountMade_new == deltaxImp_req) || ((mode != Mode.Stop) && (bytes[0] == 0) && (!params_error)))
            {
                teorxUnit += deltaxUnit_req;
                teorxImp = UnitsToImp(teorxUnit);
                sentxImp += deltaxImp_req;
                sentxUnit = ImpToUnits(sentxImp);
                mode = Mode.Stop;
                OnPinStateChanged(backPin, (int)(teorxUnit));
                return;
            }

            // берем StepCount из контроллера если нет сбоя параметров
            if (!params_error)
                stepCountMade = stepCountMade_new;
            else
            {
                stepCountMade = Math.Min(stepCountMade + (int)(vCur * (double)tmrInterval / 1000), deltaxImp_req);
                return;
            }

            //var speedPlaned = speeds[curSpeedIndex];
            if (curSpeedIndex < speeds.Count - 1)
                curSpeedIndex++;

            #region Choose vCur (vAcc or vLast or vBreak)
            // Скорость, если захотим ускориться 
            int vAcc = (new int[] { vCur + aStartImp_req, maxSpeedImp_req, IMP_PER_SECOND_MAX }).Min();

            n = (vCur / aFinishImp_req);

            // Есть три варинанта скорости на след. шаге:
            // 1. Ускориться
            // 2. Не изменить скорость
            // 3. Начать тормозить

            // Не успеем затормозить если ускоримся?
            if (stepCountMade + (vAcc + (n - 1) * vCur - (n * (n - 1) / 2) * aFinishImp_req) * (double)tmrInterval / 1000 > Math.Abs(deltaxImp_req))
            {
                // неужели  и сохранить скорость никак?
                if (stepCountMade + ((n) * vCur - (n * (n - 1) / 2) * aFinishImp_req) * (double)tmrInterval / 1000 > Math.Abs(deltaxImp_req))
                    vCur = Math.Max(vCur - aFinishImp_req, aFinishImp_req);
            }
            else vCur = vAcc;
            #endregion

            stepperProvider.SetSpeed(vCur);

            /*
            lbParams.Items.Add("Speed. Set = " + vCur + ", Planned = " + speedPlaned.ToString());
            lbParams.Items.Add("StepCountMade. Real = " + stepCountMade.ToString() + ", Planned = " + stepCountMadePlanned.ToString());
             */

            Console.WriteLine("Speed = " + vCur);

            // послать по инкрементному/декрементному пину значение, так чтобы уменьшить/увеличить 
            // текущую координату тележки. Это можно узнать от контроллера, а можно по расчетам.
            // Будем узнавать от контроллера.
            if (OnPinStateChanged != null)
                OnPinStateChanged(backPin, (int)(sentxUnit + Math.Sign(deltaxUnit_req) * ImpToUnits(stepCountMade) - (ImpToUnits(stepCountMade) > 0 ? Math.Sign(deltaxUnit_req): 0)));
        }

        public void Deinitialize()
        {
            FlushPins();
            //  Отключить Enabled
        }

        public void Initialize()
        {
            //  Подать Enabled
        }

        #endregion


        #region IConnection Members

        /// <summary>
        /// Событие: значение пина датчика изменилось
        /// </summary>
        public event PinStateChangedHandler OnPinStateChanged;

        /// <summary>
        /// Послать новое значение на виртуальный пин
        /// </summary>
        /// <param name="VirtualPinOut">вирт. пин</param>
        /// <param name="signal">значение</param>
        public void OnPin(int virtualPinOut, int signal)
        {
            if ((virtualPinOut == stopPin) && (signal == 1) && (mode != Mode.Stop))
            {
                mode = Mode.Stop;
                query = 0;                

                sentxImp += Math.Sign(deltaxUnit_req) * stepCountMade;
                sentxUnit = ImpToUnits(sentxImp);
                
                teorxUnit = (int)sentxUnit;
                teorxImp += UnitsToImp(teorxUnit);

                Console.WriteLine("Stepper.Stop begin: teorxUnit = {0}, stepCountMade = {1}", teorxUnit, stepCountMade);                

                OnPinStateChanged(backPin, (int)(teorxUnit));                
                return;
            }

            if (((virtualPinOut == queryPin) || (virtualPinOut == goPin)) && (mode != Mode.Stop))                
                throw new Exception("<StepperModule.OnPin>: Conflicted request, previous not finished");

            if (virtualPinOut == queryPin)
                query += signal;
            else if ((virtualPinOut == goPin) && (signal == 1))
            {
                SetRequestParams(query, query < 0 ? GoKind.Left : GoKind.Right);
                if (deltaxUnit_req != 0)
                {
                    TranslateRequestParams(tmrInterval);

                    //EvalSpeedsArray(Math.Abs(deltaxImp_req), tmrInterval);
                    curSpeedIndex = 0; vCur = 0;
                    stepperProvider.SetSpeed(0);
                    mode = Mode.Run;
                    stepperProvider.Go(Math.Abs(deltaxImp_req), (query < 0));
                }
                Console.WriteLine("<StepperModule.OnPin>: Stepper.Go({0})", deltaxImp_req);
                query = 0;
            }
        }

        /// <summary>
        /// Сбросить пины
        /// </summary>
        public void FlushPins()
        {
            stepperProvider.Flush();
        }

        /// <summary>
        /// Удалить соединение c шаговиком
        /// </summary>
        public void Dispose()
        {
            Deinitialize();
            stepperProvider.Dispose();
        }

        #endregion
    }
}
